﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Mapenect.Data;
using Mapenect.GesturePatterns;
using System.Runtime.InteropServices;
using OpenNI;

namespace Mapenect.GestureInterpreter
{
    class DistanceAngleMetricInterpreter : Interpreter
    {
        [DllImport("kernel32.dll")]
        public static extern bool Beep(int Frequenz, int Dauer);

        public DistanceAngleMetricInterpreter(DataContainer data, Gesture gest, float thres)
            : base(data, gest, thres)
        {
            //this.GestureDetected += new GestureEventhandler(AfterGestureDetected);
        }

        protected override bool checkForGesture()
        {
            List<double> angGest, angUser;

            MyJoint userRHand =  jointData.getUserData(user.user1)[SkeletonJoint.RightHand].GetLastElement();
            MyJoint userTorso = jointData.getUserData(user.user1)[SkeletonJoint.Torso].GetLastElement();
            MyJoint userREllbow = jointData.getUserData(user.user1)[SkeletonJoint.RightElbow].GetLastElement();
            float userDistance = getPointDistance(userRHand, userTorso);
            userDistance += getPointDistance(userREllbow, userTorso);
            angUser = getAngles(userTorso, userRHand);

            MyJoint gestRHand = pattern.Joints[SkeletonJoint.RightHand].GetLastElement();
            MyJoint gestTorso = pattern.Joints[SkeletonJoint.Torso].GetLastElement();
            MyJoint gestREllbow = pattern.Joints[SkeletonJoint.RightElbow].GetLastElement();
            float gestDistance = getPointDistance(gestRHand, gestTorso);
            gestDistance += getPointDistance(gestREllbow, gestTorso);
            angGest = getAngles(gestTorso, gestRHand);

            float tol = gestDistance * threshold;
            

            if ( (gestDistance - tol) < userDistance && (gestDistance + tol) > userDistance 
                && (angGest[0] - angGest[0] * threshold) < angUser[0] && angUser[0] < (angGest[0] + angGest[0] * threshold)
                && (angGest[1] - angGest[1] * threshold) < angUser[1] && angUser[1] < (angGest[1] + angGest[1] * threshold))
                return true;
            else
                return false;
        }

        private float getPointDistance(MyJoint torso, MyJoint armpoint)
        {
            return (float)Math.Sqrt(Math.Pow(armpoint.X - torso.X, 2.0) + Math.Pow(armpoint.Y - torso.Y, 2.0) + Math.Pow(armpoint.Z - torso.Z, 2.0));
        }

        private List<double> getAngles(MyJoint torso, MyJoint armpoint)
        {
            double x, y, z;
            List<double> res = new List<double>();
            x = Math.Abs(torso.X - armpoint.X);
            y = Math.Abs(torso.Y - armpoint.Y);
            z = Math.Abs(torso.Z - armpoint.Z);

            res.Add( Math.Atan(y / x) );
            res.Add( Math.Atan(x / z) );


            return res;
        }

        private void AfterGestureDetected(object sender, EventArgs e)
        {
            Beep(1000, 1000);
        }
    }
}
